#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, TextSubstitution
from launch_ros.substitutions import FindPackageShare
import yaml
import os


def GetConfig():
    """读取并解析USV配置文件"""
    # 改为使用脚本所在目录，而不是工作目录
    current_path = os.path.abspath('.')
    yaml_path = os.path.join(current_path, "EnviromentNode/usv_env.yaml")
    
    if not os.path.exists(yaml_path):
        raise FileNotFoundError(f"配置文件未找到: {yaml_path}")
        
    try:
        with open(yaml_path, 'r', encoding='utf-8') as file:
            yaml_content = ''.join(line for line in file if not line.strip().startswith('#'))
            return yaml.safe_load(yaml_content)
    except Exception as e:
        raise

def generate_launch_description():
    launch_desc = LaunchDescription()
    
    # 节点配置信息（直接从您的配置文件中复制）
    # node_config = {
    #     'name': 'usv_env',
    #     'isUsed': True,
    #     'package': 'xt_usv',
    #     'executable': 'xt_usv_node',
    #     'output': 'screen',
    #     'node_type': 'usv_env',
        
    #     'parameters': {
    #         'usv_type0': '20',
    #         'usv_type1': '20',
    #         'usv_type2': '1',
    #         'map_size': '400',
    #         'neibor_len': '1000' 
    #     }
    # }
    node_config = GetConfig()
    
    # 仅在启用时创建节点
    if node_config.get('isUsed', True):
        # print(f"解析了配置文件1")
        # parameters = enumerate(node_config['parameters'])
        parameters_dict = node_config['parameters']  # 直接使用配置文件中的parameters部分
        # parameters = node_config.get('parameters', {})
        # print(f"解析了配置文件1.1")
        # for param_name, param_value in parameters.items():
        #     print(f"解析了配置文件2")
        #     # 尝试转换为整数
        #     try:
        #         parameters_dict[param_name] = int(param_value)
        #     except ValueError:
        #         # 转换失败时保持为字符串
        #         parameters_dict[param_name] = param_value
        # print(f"解析了配置文件3")
        # 创建节点
        node = Node(
            package=node_config['package'],
            executable=node_config['executable'],
            name=node_config['name'],
            output=node_config['output'],
            parameters=[parameters_dict]
        )
        launch_desc.add_action(node)
        # print(f"解析了配置文件4")
        # 打印节点信息
        from launch.actions import LogInfo
        launch_desc.add_action(LogInfo(
            msg=f"启动节点: {node_config['name']} "
                f"| 包: {node_config['package']} "
                f"| 执行文件: {node_config['executable']}"
        ))
        launch_desc.add_action(LogInfo(
            msg=f"参数配置: {parameters_dict}"
        ))
    
    return launch_desc